%% Clear memory
%slCharacterEncoding('ISO-8859-1')
close all
clc
clear

%% Parameters
% Motor constants
Rs  = 160e-3; %Ohm
Ld  = 2.7e-3; %H
Lq  = 4.9e-3; %H
phi = 318e-3; %Vs
np  = 16;     % poles
J   = 0.1; % rotor inertia kgm^2

% Estimated parameters
Rs_ = Rs;
Ld_ = Ld;
Lq_ = Lq;
phi_= phi;

% Converter model
Udc = 100;   % DC-link voltage [V]
fs  = 10000; % Switching frequency [Hz]
RD  = 0.0;   % Diode resistive voltage drop [Ohm]
RT  = 0.0;   % Transistor resistive voltage drop [Ohm]
ED  = 0.0;   % Diode on state constant voltage drop [V]
ET  = 0.0;   % Transistor on state constant voltage drop [V]

% Current controller
t_rc   = 2e-3;
ac   = log(9)/t_rc;
kp_d = ac*Ld;
kp_q = ac*Lq;
ki_d = ac*ac*Ld;
ki_q = ac*ac*Lq;
Rad  = ac*Ld-Rs;
Raq  = ac*Lq-Rs;

% Speed control
w_ref = 650; %rpm
wmax  = 1000;%rpm
rpm = 600; % Initial rotor speed
t_rc  = 0.5;
aw    = log(9)/t_rc;
Ba    = aw*J/phi;
kp_w  = aw*J/phi;
ki_w  = aw*aw*J/phi;
Imax  = 52;%A
wm  = rpm/60*2*pi;
np  = 4;
wr  = wm*np/2; 

% Load Torque
ext_torque = 0;

% Speed & Position estimator
lambda= 2;

% Frequency Injection
V_inj = 100;
w_inj = 2*pi*500;
alpha_h = 6; %p 249 i boken
t_rc  = 5e-3;
alpha_e = log(9)/t_rc;
gamma = sqrt(2)*alpha_e/V_inj;
rho = alpha_e*alpha_e/V_inj;

%% Simulation
ref_step_time_w = 2.5;
ref_step_time_t = 3;
sim('model_task6',5,simset('Solver','ode45','MaxStep',1e-5));

%% Post-simulation evaluation
t       = pmsm.time - ref_step_time_w;
id      = pmsm.signals.values(:,1);
iq      = pmsm.signals.values(:,2);
theta   = pmsm.signals.values(:,3);
wm      = pmsm.signals.values(:,4);
vd      = pmsm.signals.values(:,5);
vq      = pmsm.signals.values(:,6);
Dp      = current_controller.signals.values(:,1);
Di      = current_controller.signals.values(:,2);
Qp      = current_controller.signals.values(:,3);
Qi      = current_controller.signals.values(:,4);
id_ref  = current_controller.signals.values(:,5);
iq_ref  = current_controller.signals.values(:,6);
Wp      = speed_controller.signals.values(:,1);
Wi      = speed_controller.signals.values(:,2);
Wb      = speed_controller.signals.values(:,3);
vd_     = converter.signals.values(:,1);
vq_     = converter.signals.values(:,2);

%% Plotting
figure(1)
subplot(411)
plot(t, wm*60/(2*pi));hold on
legend('w_r [rpm]')
hold off
subplot(412)
plot(t, [id iq],'');hold on
legend('i_d[A]','i_q[A]')
hold off
subplot(413)
plot(t, [vd_ vq_],'');hold on
legend('v_d[V]','v_q[V]')
hold off
subplot(414)
plot(t, [vd_.*id vq_.*iq]/1000);hold on
legend('p_d[kW]','p_q[kW]')
hold off

%% Extra
figure(2)
%n = 1000;
%plot(t,filter(ones(1,n),n,vd))
%plot(t,[Dp Di Qp Qi]);legend('Dp','Di','Qp','Qi')
plot(t,[Wp Wi Wb]);legend('Wp','Wi','Wb')
